#include "AGV_CMD.h"
#include "CRC16.h"


//************************************
// Method:    Init
// FullName:  AGV_CMD_Class::Init
// Access:    public 
// Returns:   void
// Parameter: char frame
// Parameter: char agv_add
// Parameter: char cmd
// Parameter: char subcode
// Parameter: char id
// Description: 根据参数计算指令校验码
//************************************
void AGV_CMD_Class::Init(char frame, char agv_add, char cmd, char subcode, char id)
{
	unsigned short CRC16_temp = 0;

	right[0] = busy[0] = frame;
	right[1] = busy[1] = agv_add;
	if (cmd == 0x09) //呼叫指令
	{
		right[2] = 0x09;
		busy[2] = 0x89;
	}
	else //放行指令
	{
		right[2] = busy[2] = 0x08;
	}

	right[3] = busy[3] = id;
	right[4] = busy[4] = subcode;

	CRC16_temp = CRC16_Cal((unsigned char *)right, 5);
	right[5] = CRC16_temp & 0x00FF;
	right[6] = CRC16_temp >> 8;
	CRC16_temp = CRC16_Cal((unsigned char *)busy, 5);
	busy[5] = CRC16_temp & 0x00FF;
	busy[6] = CRC16_temp >> 8;
}

void AGV_CMD_Class::find_cmd(const char * source, int length)
{
	if (TX_Flag()) //如果有发送动作，进行指令检查
	{
		if (find(source, length, right)) //寻找到了成功应答
		{
			rx_state = RIGHT_RX;
		}
		else if (find(source, length, busy)) //寻找到了繁忙应答
		{
			rx_state = BUSY_RX;
		}
		else
			rx_state = NO_RX;
	}
}

//************************************
// Method:    Check
// FullName:  AGV_CMD_Class::Check
// Access:    public 
// Returns:   int	待发送指令长度
// Parameter: char *& str 指向待发送的指令，若不需要发送指令，则指向NULL
// Description: 检查指令发送状态，返回待发送的指令
//************************************
int AGV_CMD_Class::Check(char *& str)
{
	int length = 0;	//待发送数据长度
	str = 0;
	switch (tx_state)
	{
	case READY_TX:	//准备发送
		tx_state = WAIT_TX;	//等待一个周期，确保无线模块已经从休眠状态唤醒
		//str = 0;
		//return 0;	//待发送数据长度为0
		//break;
	case NO_TX:	//无发送动作
		//str = 0;
		length = 0;//待发送数据长度为0	
		break;
	case WAIT_TX:
		rx_state = NO_RX;
		str = right;	//待发送的指令
		length = 7;
		indic = SENDING_Indic;
		tx_state = WAIT_ACK;
		retry = RETRY - 1;
		time_out = RX_TIMEOUT;
		break;
	case WAIT_ACK: //等待应答
		switch (rx_state)
		{
		case NO_RX:            //无应答
			if (time_out == 0) //超时
			{
				if (retry == 0) //重试次数已达到上限
				{
					tx_state = ERROR_ACK;
				}
				else //重试次数未到达上限
				{
					str = right;	//待发送的指令
					length = 7;
					indic = SENDING_Indic;
					retry--;
					time_out = RX_TIMEOUT;
				}
			}
			else
			{
				time_out--;
			}
			break;
		case RIGHT_RX: //接收到成功应答
			tx_state = SUCCESS_ACK;
			indic = RIGHT_Indic;
			break;
		case BUSY_RX:
			tx_state = SUCCESS_ACK;
			indic = BUSY_Indic;
			break;
		default:
			break;
		}
		break;
	case SUCCESS_ACK: //接收应答成功
		rx_state = NO_RX;
		tx_state = NO_TX;
		break;
	case ERROR_ACK: //接收应答失败
		tx_state = NO_TX;
		indic = FAILURE_Indic;
		break;
	default:
		break;
	}
	return length;
}

bool AGV_CMD_Class::find(const char * source, int length, const char * cmd)
{
	int i = 0, j = 0;
	for (i = 0; i < length - 6; i++)
	{
		if ((*source) == (*cmd))
		{
			for (j = 1; j < 7; j++)
			{
				if (source[j] != cmd[j])
				{
					break;
				}
			}
			if (j == 7)
			{
				return true;
			}
		}
		source++;
	}
	return false;
}
